"""Implements classes for data collection in Carla."""

# %%
# The following boilerplate is required if .egg is not installed
# See: https://carla.readthedocs.io/en/latest/build_system/
import glob
import os
import sys
import re
import random
import queue
from collections import deque
import time
import numpy as np

# from carlasim.groundtruth import GroundTruthExtractor
# from carlasim.carla_tform import Transform
# from agents.navigation.behavior_agent import BehaviorAgent

# try:
#     sys.path.append(glob.glob('./carla-*%d.%d-%s.egg' % (
#         sys.version_info.major,
#         sys.version_info.minor,
#         'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
# except IndexError:
#     pass
import carla

# %% ================= Global function =================


# %% ================= Geo2Location =================

# TODO: Replace this with pyproj?


class Geo2Location(object):
    """
    Helper class for homogeneous transform from geolocation

    This class is used by GNSS class to transform from carla.GeoLocation to carla.Location.
    This transform is not provided by Carla, but it can be solved using 4 chosen points.
    Note that carla.Location is in the left-handed coordinate system.
    """

    def __init__(self, carla_map):
        """ Constructor method """
        self._map = carla_map
        # Pick 4 points of carla.Location
        loc1 = carla.Location(0, 0, 0)
        loc2 = carla.Location(1, 0, 0)
        loc3 = carla.Location(0, 1, 0)
        loc4 = carla.Location(0, 0, 1)
        # Get the corresponding carla.GeoLocation points using carla's transform_to_geolocation()
        geoloc1 = self._map.transform_to_geolocation(loc1)
        geoloc2 = self._map.transform_to_geolocation(loc2)
        geoloc3 = self._map.transform_to_geolocation(loc3)
        geoloc4 = self._map.transform_to_geolocation(loc4)
        # Solve the transform from geolocation to location (geolocation_to_location)
        l = np.array([[loc1.x, loc2.x, loc3.x, loc4.x],
                      [loc1.y, loc2.y, loc3.y, loc4.y],
                      [loc1.z, loc2.z, loc3.z, loc4.z],
                      [1, 1, 1, 1]], dtype=float)
        g = np.array([[geoloc1.latitude, geoloc2.latitude, geoloc3.latitude, geoloc4.latitude],
                      [geoloc1.longitude, geoloc2.longitude,
                          geoloc3.longitude, geoloc4.longitude],
                      [geoloc1.altitude, geoloc2.altitude,
                          geoloc3.altitude, geoloc4.altitude],
                      [1, 1, 1, 1]], dtype=float)
        # Tform = (G*L^-1)^-1
        self._tform = np.linalg.inv(g.dot(np.linalg.inv(l)))
        # self._tform = np.linalg.inv(g).dot(l)

    def transform(self, geolocation):
        """
        Transform from carla.GeoLocation to carla.Location (left_handed z-up).

        Numerical error may exist. Experiments show error is about under 1 cm in Town03.
        """
        geoloc = np.array(
            [geolocation.latitude, geolocation.longitude, geolocation.altitude, 1])
        loc = self._tform.dot(geoloc.T)
        return carla.Location(loc[0], loc[1], loc[2])

    def get_matrix(self):
        """ Get the 4-by-4 transform matrix """
        return self._tform
    def set_matrix(self,matrix):
        """ set the 4-by-4 transform matrix """
        self._tform = matrix

# %% ================= Sensor Base =================


def save_numpy(array,name):
    path_txt=f'./transferMatric_G2L/{name}.txt'
    path_np=f'./transferMatric_G2L/{name}.npy'
    np.savetxt(path_txt, array)
    np.save(path_np, array)

def load_numpy_txt(name):
    path_txt=f'./transferMatric_G2L/{name}.txt'
    # path_np=f'./transferMatric_G2L/{name}.npy'
    return np.loadtxt(path_txt)
    # np.load(path_np)

def load_numpy_np(name):
    # path_txt=f'./transferMatric_G2L/{name}.txt'
    path_np=f'./transferMatric_G2L/{name}.npy'
    # np.load(path_txt)
    return np.load(path_np)

if __name__ == '__main__':
    # >>>>>获取地图并计算转换矩阵
    client=carla.Client('127.0.0.1',2000)
    # world=client.load_world('Town07')
    world=client.get_world()
    map=world.get_map()

    
    # >>>>>获取车辆当前的gnss并打印信息:
    actors=world.get_actors()
    gnss_actor_list = actors.filter('*gnss*') #找到gnss
    gnss_actor = gnss_actor_list[0]
    print(gnss_actor)
    car_longitude=0.0
    car_latitude=0.0
    car_altitude=0.0
    car_transform=None
    def callback(data):
        global car_longitude,car_latitude,car_altitude
        print(f">>>>>>gnss传感器geo坐标值: longitude 经度:{data.longitude},latitude 维度:{data.latitude},altitude 海拔:{data.altitude}")
        car_longitude=data.longitude
        car_latitude=data.latitude
        car_altitude=data.altitude
        car_transform=data.transform
        print(f">>>>>>gnss传感器中的carla坐标值 x:{car_transform.location.x},y:{car_transform.location.y},z:{car_transform.location.z}")
    gnss_actor.listen(lambda data: callback(data))
    

    times = 0
    while times < 1:
        print('----------------------------------------------------')
        times +=1
        world.tick()
        #
        # >>>>>>获取车辆的carla.Location并打印信息:
        vehicle_actor_list=actors.filter('*vehicle*')
        vehicle_actor=vehicle_actor_list[0]
        vehicle_location=vehicle_actor.get_location()
        print(f">>>>>>API读取carla坐标值:vehicle location x:{vehicle_location.x},y:{vehicle_location.y},z:{vehicle_location.z}")
        
        # >>>>>carla.GeoLocation -> carla.Location:  
        # 使用Geo2Location类转换
        g2l_obj=Geo2Location(map) #通过加入地图map计算转换矩阵
        trans_matrix=g2l_obj.get_matrix() #转换矩阵
        save_numpy(trans_matrix, 'town07')
        matrix_val=load_numpy_txt('town07')
        g2l_obj.set_matrix(matrix_val)
        
        car_gnss=carla.GeoLocation(longitude=car_longitude,latitude=car_latitude,altitude=car_altitude)
        car_location=carla.Location()
        car_location=g2l_obj.transform(car_gnss)
        print(f'>>>>>>离线转换的carla坐标值:car_location x:{car_location.x},y:{car_location.y},z:{car_location.z}')
        

        # >>>>>carla.Location -> carla.GeoLocation:  
        # 通过carla中自带的函数转换  
        car_geo=map.transform_to_geolocation(vehicle_location)
        # print(f'>>>>>>carla自带函数转换值:longitude:{car_geo.longitude},latitude:{car_geo.latitude},altitude:{car_geo.altitude}')

        print('----------------------------------------------------')